/* USER CODE BEGIN Header */
/**
  ******************************************************************************
  * File Name          : flow.cpp
  * Description        : 
  ******************************************************************************
  * Function           :
  *
  ******************************************************************************
  * Author             : Zhiping Fu
  * Creation Date      : Nov 12, 2020
  ******************************************************************************
  */
/* USER CODE END Header */
#include "flow.hpp"
#include "string.h"

FLOW flow(USART3,(char *)"flow");

//void USART3_IRQHandler(void)
//{
//	if(LL_USART_IsActiveFlag_IDLE(USART3))
//	{
//		LL_USART_ClearFlag_IDLE(USART3);
//		LL_DMA_DisableStream(DMA1,LL_DMA_STREAM_1);
//		LL_DMA_ClearFlag_DME1(DMA1);
//		LL_DMA_ClearFlag_HT1(DMA1);
//		LL_DMA_ClearFlag_TC1(DMA1);
//		LL_DMA_ClearFlag_TE1(DMA1);
//		LL_DMA_ClearFlag_FE1(DMA1);
//		flow.RxDataSize = FLOW_RX_LEN - LL_DMA_GetDataLength(DMA1, LL_DMA_STREAM_1);
//
//		do{
//			if(flow.RxRawDat[1] != 0xfd) break;
//			if(flow.LockRx == HAL_LOCKED)break;
//			flow.LockRx = HAL_LOCKED;
//			memcpy(flow.RxDat,flow.RxRawDat,flow.RxDataSize);
//			flow.RxDat[flow.RxDataSize]=0;
//			flow.LockRx = HAL_UNLOCKED;
//			flow.RxFlag = true;   //收到完整一帧
//
//			BaseType_t YieldRequired = xTaskResumeFromISR(flowTaskHandle);
//			if(YieldRequired==pdTRUE)
//			{
//				/*如果函数xTaskResumeFromISR()返回值为pdTRUE，那么说明要恢复的这个
//				任务的任务优先级等于或者高于正在运行的任务(被中断打断的任务),所以在
//				退出中断的时候一定要进行上下文切换！*/
//				portYIELD_FROM_ISR(YieldRequired);
//			}
//
//		}while(0);
//
//		LL_DMA_SetDataLength(DMA1, LL_DMA_STREAM_1, FLOW_RX_LEN);
//		LL_DMA_EnableStream(DMA1, LL_DMA_STREAM_1);
//	}
//}

void FLOW::flow_Init(void)
{
	LockRx = HAL_UNLOCKED;
	RxFlag = false;
	RxDataSize = 0;
	executionTime_us = 0;
	Sta = STA_INI;
	Err = ERR_NONE;

	/* 配置接收DMA */
	LL_DMA_DisableStream(DMA1,LL_DMA_STREAM_1);
	LL_DMA_SetPeriphAddress(DMA1, LL_DMA_STREAM_1, (uint32_t)&huart->RDR);
	LL_DMA_SetMemoryAddress(DMA1, LL_DMA_STREAM_1, (uint32_t)RxRawDat);
	LL_DMA_SetDataLength(DMA1, LL_DMA_STREAM_1, FLOW_RX_LEN);
	LL_DMA_EnableStream(DMA1, LL_DMA_STREAM_1);
	/* 配置接收DMA */

	LL_USART_EnableDMAReq_RX(huart);
	LL_USART_ClearFlag_IDLE(huart);
	LL_USART_EnableIT_IDLE(huart);
	Sta = STA_RUN;
	osDelay(100);
}

void FLOW::flow_Update(void)
{
	startTimerLast = startTimer;
	getTimer_us(&startTimer);
	cycleTime_us = startTimer - startTimerLast;

	if(LockRx == HAL_LOCKED)return;
	LockRx = HAL_LOCKED;
	do{
		uint8_t num = RxDat[2];
		TxSum = 0;
		for(uint8_t i=0; i<num-1; i++) TxSum += RxDat[i];
		if(TxSum != RxDat[num-1])break;		//和校验 判断sum
		switch(RxDat[0])
		{
		case 0xf1:	//光流得到的加速度数据
			memcpy(temp.uc,&RxDat[3],4);
			height.height = temp.fp/100.0f;
			memcpy(temp.uc,&RxDat[7],4);
			height.speedZ = temp.fp/100.0f;
			memcpy(temp.uc,&RxDat[11],4);
			flow.flowx = temp.fp/1000.0f;
			memcpy(temp.uc,&RxDat[15],4);
			flow.flowy = temp.fp/1000.0f;
			memcpy(temp.uc,&RxDat[19],4);
			flow.flowx2 = temp.fp/100.0f;
			memcpy(temp.uc,&RxDat[23],4);
			flow.flowy2 = temp.fp/100.0f;
			xQueueOverwrite(queueFlow,&flow);
			xQueueOverwrite(queueHeight,&height);
			break;
		case 0xf2:	//光流数据
			memcpy(temp.uc,&RxDat[3],4);
			flow.flowx = temp.fp;
			memcpy(temp.uc,&RxDat[7],4);
			flow.flowy = temp.fp;
			memcpy(temp.uc,&RxDat[11],4);
			flow.timestamp = temp.ui;
			xQueueOverwrite(queueFlow,&flow);
			break;

		case 0xf3:	//高度数据
			memcpy(temp.uc,&RxDat[3],4);
			height.height = temp.fp;
			memcpy(temp.uc,&RxDat[7],4);
			height.timestamp = temp.ui;
			xQueueOverwrite(queueHeight,&height);
			break;

		}
	}while(0);
	LockRx = HAL_UNLOCKED;
	getTimer_us(&stopTimer);

	executionTime_us = stopTimer - startTimer;
}


extern "C" void flow_main(void *argument)
{
	flow.flow_Init();
	for(;;)
	{
		vTaskSuspend(flowTaskHandle);
		flow.flow_Update();
	}
}


/************************ (C) COPYRIGHT Longmen Drone Team *****END OF FILE****/
